#include "feexport.h"

FEexport::FEexport(QString ProjName, QString EventName)
{
    projName=ProjName;
    eventName=EventName;
    tankRadius=310;  //in millimeters

}

int FEexport::init(int inc)
{
    increments=inc; // number of angular segments along the lower half of the tank

    //formulate the laser line sequence parameter file name
    QString laserYML = QString(projName + "/LaserLineSequence.yml");
    fs.open(laserYML.toStdString(),FileStorage::READ);
    if (!fs.isOpened())
    {
        qDebug("Failed to read the laser line sequence parameter file.");
        return 1;
    }

    fs["LaserLineSequence"] >> lasers;
    if( lasers.empty() ) {
        qDebug("Not a laser line sequence parameter file.");
        return 1;
    }
    fs.release();

    //generate the initial grid point positions
    grid=Mat(lasers.rows,increments+1, CV_32FC3);  //define the grid X, Y, Z
    Mat_<Vec3f> _grid=grid; //access to the data for easy notation
    Mat_<float> _lasers=lasers;

    for(int x=0;x<lasers.rows;x++)
        for(int n=0;n<increments+1;n++) {
            _grid(x,n)[0] =_lasers(x); //X value = Laser line position
            float angle=n * M_PI / increments;
            _grid(x,n)[1] = -tankRadius*cos(angle);  //Y value
            _grid(x,n)[2] = -tankRadius*sin(angle);  //Z depth
        }

     outFile.setFileName(eventName+"/ProcessedResults/out.fem");
     outFile.open(QIODevice::WriteOnly);
     QTextStream out(&outFile);
     out.setRealNumberNotation(QTextStream::FixedNotation);  //fixed notation (I hate scientific notation)
     out.setRealNumberPrecision(1); // one decimal place


     out << "$$\n$$ GRID Data\n$$\n"; //write the grid header
     for(int j=0;j<grid.rows;j++)
         for(int k=0;k<_grid.cols;k++)
             out << "GRID," << (j+1)*100+k << ",," << _grid(j,k)[0] << ","
                 << _grid(j,k)[1] << "," << _grid(j,k)[2] << "\n";

     out << "$$\n$$ PLOTEL3 Elements\n$$\n"; //write triangle element header
     int elementNum=1;
     for(int j=0;j<grid.rows-1;j++)
         for(int k=0;k<_grid.cols-1;k++) {
             int n1=(j+1)*100+k; //node1
             int n2=n1+1; //node2
             int n3=n1+100; //node3
             int n4=n2+100; //node4

             out << "PLOTEL3," << elementNum++ << "," << n1 << ","
                 << n2 << "," << n4 << "\n";
             out << "PLOTEL3," << elementNum++ << "," << n1 << ","
                 << n4 << "," << n3 << "\n";

         }
     outFile.close();

     lowerY=Mat(grid.cols,1,CV_32F);
     upperY=Mat(grid.cols,1,CV_32F);

     //lower end
     lowerY(0,0)= _grid(0,0)[1];
     upperY(0,0) =  _grid(0,0)[1] + (_grid(0,1)[1] - _grid(0,0)[1])/2.0;

     //middle
     for(int n=1;n<grid.cols-1;n++) {
         lowerY(n,0)=  _grid(0,n)[1] - (_grid(0,n)[1] - _grid(0,n-1)[1])/2.0 ;  // lower = y[n]-(y[n]-y[n-1])/2
         upperY(n,0) =  _grid(0,n)[1] + (_grid(0,n+1)[1] - _grid(0,n)[1])/2.0;   // upper = y[n]+(y[n+1]-y[n])/2
     }

     //upper end
     lowerY(grid.cols-1,0) =  _grid(0,grid.cols-1)[1] - (_grid(0,grid.cols-1)[1] - _grid(0,grid.cols-2)[1])/2.0;
     upperY(grid.cols-1,0) =  _grid(0,grid.cols-1)[1];


     //     for(int x=0;x<lasers.rows;x++) {
//         lowerX.push_back(_grid(x,0)[0]-10.0);
//         upperX.push_back(_grid(x,0)[0]+10.0);
//     }
     lowerX=Mat(lasers.rows,1,CV_32F);
     upperX=Mat(lasers.rows,1,CV_32F);


     for(int x=0;x<lasers.rows;x++) {
         lowerX(x,0) =_grid(x,0)[0]-10.0;
         upperX(x,0) = _grid(x,0)[0]+10.0;
     }


    return 0;
}

int FEexport::read() {

    QString dataFile0 = eventName+"/ProcessedResults/cam0_3Dpts.yml.gz";
    QString dataFile1 = eventName+"/ProcessedResults/cam1_3Dpts.yml.gz";

    emit status(2); //show some progress
    fsCam0.open(dataFile0.toStdString(), FileStorage::READ);
    if (!fsCam0.isOpened())
    {
        qDebug() << "Failed to open data file: "<< dataFile0;
        return 1;

    } else {

        // setup sequence & iterator
        frames = fsCam0["Frames"];
        it = frames.begin();
        frameTotal = it.remaining;

    }
    emit status(4); //show some progress
    fsCam1.open(dataFile1.toStdString(), FileStorage::READ);
    if (!fsCam1.isOpened())
    {
        qDebug() << "Failed to open data file: "<< dataFile1;
        return 1;

    } else {
        // setup sequence & iterator
        frames1 = fsCam1["Frames"];
        it1 = frames1.begin();

    }

    //read time code
    QString timeCodeFile = eventName+"/VideoFiles/VideoInfo.yml";
    fs.open(timeCodeFile.toStdString(),FileStorage::READ);

    fs["TimeCode"] >> timecode;
    fs.release();
    itTimecode=timecode.begin();

//    cout << "[";
//    for(int i=0;i<timecode.size();i++)  {cout << timecode[i] << ",";}
//    cout << "]\n";

    return 0;
}


void FEexport::write() {

    outFile.setFileName(eventName+"/ProcessedResults/out.hwascii");
    outFile.open(QIODevice::WriteOnly);
    QTextStream out(&outFile);
    out.setRealNumberNotation(QTextStream::FixedNotation);  //fixed notation (I hate scientific notation)
    out.setRealNumberPrecision(1); // one decimal place

    // write the file header
    out << "ALTAIR ASCII FILE \n";
    out << "$TITLE = Slosh Test Transient Results\n";
    out << "$SUBCASE = 1    Subcase 1\n";
    out << "$BINDING = NODE\n";
    out << "$COLUMN_INFO = ENTITY_ID\n";
    out << "$RESULT_TYPE = Displacement(v), Depth (s)\n";


    int timecode=0;  // dummy timecode until I get the read-in values

    Mat output(grid.size(),grid.type(),Scalar(0.,0.,0.)); //the output, same type & size as grid
    Mat output_save; //a Mat to store cam0 values for averaging with cam1 later

    Mat_<Vec3f> _grid=grid; //access to the data for easy notation
    Mat_<Vec3f> _output=output; //access to the data for easy notation


    Mat haveData(output.size(),CV_8U);  //used to note which grids actually have data.
    Mat_<uchar> _haveData=haveData; //access to the data for easy notation
    Mat haveData_save; //a Mat to store the cam0 haveData for averaging with cam1 later

    Mat visTriangles(0,0,CV_32FC3); //zero element, 3-float Mat
    QString visTri(eventName + "/ProcessedResults/visTriangles.yml");
    fs.open(visTri.toStdString(),FileStorage::WRITE);
    fs << "Frames" << "["; //start sequence

    bool flipper=true;
    do{
        Mat closestY(output.size(),CV_32FC1,Scalar(1000.0));  //keep track so we can keep the closest Y.  Initialize with big number.
        Mat_<Vec<float,1> > _closestY=closestY; //access to the data for easy notation

        //read in the points
        if(flipper) {
            (*it)["ExportedPoints"] >> points;
            it++;// increment the iterator
            flipper = !flipper;
            haveData=Mat(output.size(),CV_8U,Scalar(0));  //initialize with zero
            _haveData=haveData;
            _output=output *= 0;  //zero & assign to quick reference
        } else {
            (*it1)["ExportedPoints"] >> points;
            it1++;// increment the iterator
            flipper = !flipper;
            output.copyTo(output_save); //copy cam0 results
            _output=output *= 0;  //zero & assign to quick reference
            haveData.copyTo(haveData_save);
            haveData=Mat(output.size(),CV_8U,Scalar(0));  //initialize with zero
            _haveData=haveData;

        }

        //make sure we get good data
        if (points.empty()){
            qDebug()<< "points0 is empty!!";
        }

        Mat_<Vec3f> _points=points;
        //iterate through the data
        for (int i=0; i<points.rows; i++) {
            float x,y,z;
            x=_points(i)[0];
            y=_points(i)[1];
            z=_points(i)[2];



            //iterate through the grid points
            for(int j=0;j<lasers.rows;j++) {
                if (x < upperX(j,0) && x > lowerX(j,0))
                    for(int k=0;k<output.cols;k++)
                        if(y<upperY(k,0) && y>lowerY(k,0)) {
                            //calculate the magnitude of the Y-distance
                            float distY=fabs(_grid(j,k)[1]-y);
                            if(distY<_closestY(j,k)[0]) {
                                _closestY(j,k)[0]=distY; // set it as the closest one
                                //_output(j,k)[0]=x;
                                _output(j,k)[1]=y;
                                _output(j,k)[2]=z;

                                _haveData(j,k) = 1;  //flag this grid as having data
                            }
                        }
            }
        }

        /* turn this off for now
        //iterate through the grid and fill in holes of missing data (average of neighbors that do have data)
        for(int j=0;j<lasers.rows;j++)
            for(int k=0;k<output.cols;k++)
                if (! _haveData(j,k)) { //check if this grid point does not have data

                    float zVal=0.0;
                    int numNeighbors=0;
                    bool forward=true,back=true, left=true,right=true;

                    //bounds checks
                    if(j == 0)  //front row
                        back=false;
                    else if(j == lasers.rows-1)  //back row
                        forward=false;
                    if(k == 0) //far left
                        left=false;
                    else if (k == output.cols-1) //far right
                        right=false;

                    if(forward)
                        if(_haveData(j+1,k)) {
                            numNeighbors++;
                            zVal+=_output(j+1,k)[2]; // add the z value
                        }
                    if(back)
                        if(_haveData(j-1,k)) {
                            numNeighbors++;
                            zVal+=_output(j-1,k)[2]; // add the z value
                        }
                    if(left)
                        if(_haveData(j,k-1)) {
                            numNeighbors++;
                            zVal+=_output(j,k-1)[2]; // add the z value
                        }
                    if(right)
                        if(_haveData(j,k+1)) {
                            numNeighbors++;
                            zVal+=_output(j,k+1)[2]; // add the z value
                        }
                    if(numNeighbors)
                        _output(j,k)[2]=zVal/numNeighbors; //set the z value of this grid to be the average of neighbors that have valid data
                    else {

                        // this will return the output node to the tank wall if no neighbors exist for averaging
                        if(false){  //turned off for now.  Without it, the previous value gets held, which looks better.
                        _output(j,k)[2]=_grid(j,k)[2];
                        _output(j,k)[1]=_grid(j,k)[1];
                        }
                    }
                }
        turned off filling holes */


        if(flipper) {
            //out << "$TIME = " << (float) timecode /1000.0 << " sec\n";
            //timecode+=50; //add fifty milliseconds.  This will get replaced when I get good values for the timecode.
            int time=*itTimecode;  //cam0 time
            itTimecode++; //iterate to the next (cam1 time)
            time += *itTimecode; // add cam1 to cam0
            itTimecode++; //iterate to next time code (cam0 for next loop)
            time /=2; // average of the two cameras
            out.setRealNumberPrecision(3); //bump precision so timecode has 3 digits
            out << "$TIME = " << (float) time /1000.0 << " sec\n";  // Convert to seconds from millsecond: divide by 1000
            out.setRealNumberPrecision(1); //back to one decimal

            haveData = haveData + haveData_save;  //combine both cam0 & cam1.  Result is 1 if one cam, 2 if both cams.

            Mat_<Vec3f> _output_save=output_save; //access to the data for easy notation
            for(int j=0;j<lasers.rows;j++)
                for(int k=0;k<_output.cols;k++)
                    if(_haveData(j,k)){
                        //average of Y(i=1) & Z(i=2) data from both cams
                        for(int i=1;i<3;i++) _output(j,k)[i] = (_output(j,k)[i] + _output_save(j,k)[i])/ (float)_haveData(j,k);//average of cam0 & cam1 (divides by one if only one cam, divides by two if both)

                        float dispX=0.0;  //no change to X
                        float dispY=_output(j,k)[1] - _grid(j,k)[1];
                        float dispZ=_output(j,k)[2] - _grid(j,k)[2];
                        out << (j+1)*100+k << " " << dispX <<" " << dispY << " "<< dispZ << " " << _output(j,k)[2] << "\n";
                    }
            //export the visual triangles
            for(int j=0;j<lasers.rows-1;j++)
                for(int k=0;k<output.cols-1;k++)
                    if(_haveData(j,k) && _haveData(j+1,k+1)) {
                        if(_haveData(j,k+1)) {  //triangle 1
                            visTriangles.push_back(Point3f(_grid(j,k)[0],_output(j,k)[1],_output(j,k)[2]));
                            visTriangles.push_back(Point3f(_grid(j,k+1)[0],_output(j,k+1)[1],_output(j,k+1)[2]));
                            visTriangles.push_back(Point3f(_grid(j+1,k+1)[0],_output(j+1,k+1)[1],_output(j+1,k+1)[2]));
                        }
                        if(_haveData(j+1,k)) { //triangle 2
                            visTriangles.push_back(Point3f(_grid(j,k)[0],_output(j,k)[1],_output(j,k)[2]));
                            visTriangles.push_back(Point3f(_grid(j+1,k+1)[0],_output(j+1,k+1)[1],_output(j+1,k+1)[2]));
                            visTriangles.push_back(Point3f(_grid(j+1,k)[0],_output(j+1,k)[1],_output(j+1,k)[2]));
                        }
                    }

            fs << "{";
            fs << "TimeCode" << time;
            fs << "Triangles" << visTriangles;
            fs << "}";
            visTriangles.release();

        } // flipper



    emit status(100 - it.remaining * 100 / frameTotal);
    } while(it1.remaining>1);

    fs << "]"; //end sequence
    fs.release(); //close file

    outFile.close();
    emit done();

}

void FEexport::doIt() {
    int readOK=read();
    if (readOK == 0)
        write();
}
